#include "v4l2.h"


V4L2::V4L2(QObject *parent) : QThread(parent)
{
    start();
}



void V4L2::run()
{
    int camerafd;
    int ret;

    //打开摄像头
    camerafd=open("/dev/video7",O_RDWR);
    if(camerafd==-1)
    {
        perror("open camera failed!\n");
        return ;
    }
    //设置采集格式
    struct v4l2_format format;
    memset(&format, 0, sizeof format);
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    format.fmt.pix.width = 640;
    format.fmt.pix.height = 480;
    format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    format.fmt.pix.field = V4L2_FIELD_NONE;
    ret=ioctl(camerafd,VIDIOC_S_FMT, &format);
    if(ret==-1)
    {
        perror("设置采集格式失败!\n");
        return ;
    }
    //申请缓存
    struct v4l2_requestbuffers req;
    memset(&req, 0, sizeof req);
    req.count = 4;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;
    if(ioctl(camerafd,VIDIOC_REQBUFS, &req)== -1)
    {
        perror("申请缓存失败!\n");
        return ;
    }
    //分配你刚才申请的缓冲块 --- 队列中设置了4个缓冲区 pbuf[i],
    struct usrbuf *pbuf= (struct usrbuf *)calloc(4,sizeof(struct usrbuf));
    for(int i = 0; i < 4; i++)
    {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof buf);
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;
        if (ioctl(camerafd,VIDIOC_QUERYBUF,&buf) == -1)
        {
            perror("分配缓存失败!\n");
            return ;
        }
        // 四个缓冲区是直接映射camerafd数据的
        pbuf[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED,
               camerafd, buf.m.offset);
        pbuf[i].length=buf.length;
     }
    //入队VIDIOC_QBUF
    for (int i = 0; i < 4; i++)
    {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof buf);
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;
        if (ioctl(camerafd,VIDIOC_QBUF,&buf) == -1)
        {
            perror("入队失败!\n");
            return ;
        }
    }
    //开始采集VIDIOC_STREAMON
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if(ioctl(camerafd, VIDIOC_STREAMON, &type) == -1)
        {
        perror("开始采集失败!\n");
        return ;
    }

    int lcdmem[800*480*4] = {0};
    unsigned char* rgb = (unsigned char*)malloc(640*480*3);
    struct timeval timeout;
    timeout.tv_sec = 1;
    timeout.tv_usec = 0;
    while(1)
    {struct timeval timeout;
        timeout.tv_sec = 1;
        timeout.tv_usec = 0;
        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(camerafd, &fds);
        int r = select(camerafd + 1, &fds, 0, 0, &timeout); // 只要这个摄像头文件有动静就会结束阻塞
        for(int  i=0; i<4; i++)
        {

            struct v4l2_buffer buf;
            memset(&buf, 0, sizeof buf);
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            buf.memory = V4L2_MEMORY_MMAP;
            buf.index=i;
            if (ioctl(camerafd, VIDIOC_DQBUF, &buf) == -1)// 读不出数据出错了 VIDIOC_DQBUF
            {
                perror("出队失败!\n");

                continue ;
            }
            if(ioctl(camerafd, VIDIOC_QBUF, &buf) == -1)//  写不进数据
            {
                perror("入队失败!\n");

                continue ;
            }
            // 正常数据
            // 把V4L2输出的YUYV格式数据转换成RGB
            yuyv2rgb0((unsigned char *)pbuf[i].start,rgb,640,480);

            if(isOn)
            {
                emit sigImage(rgb);
                msleep(30);
            }
        }

    }
}

void V4L2::slotOnOff(bool onOff)
{
    qDebug() << (onOff?"open":"false");
    isOn = onOff;
}








int V4L2::yuyv2rgb(int y, int u, int v)
{
     unsigned int pixel24 = 0;
     unsigned char *pixel = (unsigned char *)&pixel24;
     int r, g, b;
     static int  ruv, guv, buv;

     // 色度
     ruv = 1596*(v-128);
     guv = 391*(u-128) + 813*(v-128);
     buv = 2018*(u-128);

    // RGB
     r = (1164*(y-16) + ruv) / 1000;
     g = (1164*(y-16) - guv) / 1000;
     b = (1164*(y-16) + buv) / 1000;

     if(r > 255) r = 255;
     if(g > 255) g = 255;
     if(b > 255) b = 255;
     if(r < 0) r = 0;
     if(g < 0) g = 0;
     if(b < 0) b = 0;

     pixel[0] = r;
     pixel[1] = g;
     pixel[2] = b;

     return pixel24;
}


int V4L2::yuyv2rgb0(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height)
{
     unsigned int in, out;
     int y0, u, y1, v;
     unsigned int pixel24;
     unsigned char *pixel = (unsigned char *)&pixel24;
     unsigned int size = width*height*2;

     for(in = 0, out = 0; in < size; in += 4, out += 6)
     {
    // YUYV
          y0 = yuv[in+0];
          u  = yuv[in+1];
          y1 = yuv[in+2];
          v  = yuv[in+3];

          pixel24 = yuyv2rgb(y0, u, v); // RGB
          rgb[out+0] = pixel[0];
          rgb[out+1] = pixel[1];
          rgb[out+2] = pixel[2];

          pixel24 = yuyv2rgb(y1, u, v);// RGB
          rgb[out+3] = pixel[0];
          rgb[out+4] = pixel[1];
          rgb[out+5] = pixel[2];

     }
     return 0;
}
